#include "control.h"
#include "servo.h"
#include "math.h"

uint8_t runMode = 0;        //0停止 1运行
uint32_t THRESHOLD = 50;   //阈值
uint32_t targetSpeed = 150; //Unit: mm/s
double actrulPosition = 0;  //Unit: cm
uint32_t angleKp = 700000;
uint32_t angleKi = 0;
uint32_t angleKd = 7000000;
PIDControl anglePID; //转向环
uint8_t AGV_Data[8] = {0};
int16_t location = 45;
int dirSet = 0;

uint8_t runLog[5000] = {0};

int carpos = 0;

uint32_t FinalTime=0;
uint32_t stopPassTime=0;

void readFlash()
{
    //STMFLASH_Read(FLASH_SAVE_ADDR, (uint16_t*)datatemp, SIZE);
}

void writeFlash()
{
    //STMFLASH_Write(FLASH_SAVE_ADDR, (uint16_t*)TEXT_Buffer, SIZE);
}

static uint8_t spMode;
static uint8_t lastspMode;

static uint32_t runTime;
static uint32_t spTime;

//运行初始化
void runInit()
{
    spMode = 0;
    runTime = 100;
    spTime = 0;
    PID_Init(&anglePID);
    anglePID.target = 45;
    anglePID.Kp = angleKp / 100000.0;
    anglePID.Ki = angleKi / 100000.0;
    anglePID.Kd = angleKd / 100000.0;
    anglePID.actrul = 0;
    //状态采集初始化
    //stateInit();
}

uint8_t getLocation(void)
{
    uint8_t i = 0;
    uint8_t max1 = 0, max2 = 0;
    uint8_t maxno1 = 0, maxno2 = 0;
    int8_t err = 0;
    for (i = 0; i < 8; i++)
    {
        if (AGV_Data[i] > max1)
        {
            max1 = AGV_Data[i];
            maxno1 = i;
        }
    }
    for (i = 0; i < 8; i++)
    {
        if (i == maxno1)
            continue;
        if (AGV_Data[i] > max2)
        {
            max2 = AGV_Data[i];
            maxno2 = i;
        }
    }
    if (maxno2 > maxno1)
        err = AGV_Data[maxno2] - AGV_Data[maxno1];
    else
        err = AGV_Data[maxno1] - AGV_Data[maxno2];
    if (max1 > THRESHOLD && max2 > THRESHOLD)
    {
        location = (8 - (maxno1 > maxno2 ? maxno2 : maxno1)) * 10 - 5 - err / 10;
        return 0;
    }
    if (location >= 50)
        location = 85;
    if (location <= 40)
        location = 5;
    return 1;
}

uint8_t switchPos()
{
    uint8_t Pos;
    int i = 0;
    for (i = 0; i < 8; i++)
    {
        if (AGV_Data[i] > THRESHOLD)
            Pos |= 0x01U << i;
    }
    return Pos;
}

static uint8_t pospos = 0;
static uint8_t stopTimes = 0;
#define SWITCHPOS pospos = switchPos()
#define POS(x) (pospos & (0x01U << x))
#define ALL(x) (x == 0xFFU)
#define NON(x) (x == 0x00U)

//#define MID ((switchPos()&0x99U)&&(~switchPos()&0x42U))
//#define LFT ((switchPos()&0x70U)&&(~switchPos()&0x8FU))
//#define RIT ((switchPos()&0x07U)&&(~switchPos()&0xF8U))
//#define SID ((switchPos()&0x81U)&&(~switchPos()&0x7EU))
//#define BIC ((switchPos()&0x1FU)&&(~switchPos()&0xE0U))

#define MID(x) (((0x18U & x) == 0x18U) && ((0x81U & ~x) == 0x81U))
#define RIT(x) (((0x02U & x) == 0x02U) && ((0xF0U & ~x) == 0xF0U))
#define LFT(x) (((0x40U & x) == 0x40U) && ((0x0FU & ~x) == 0x0FU))
#define SID(x) (((0x81U & x) == 0x81U) && ((0x18U & ~x) == 0x18U))
#define BIC(x) (((0x0EU & x) == 0x0EU) && ((0x70U & ~x) == 0x70U))

#define TIMERROR (runTime - spTime)

typedef enum
{
    NORMOL = 0,
    STOP = 1,
    CROSS,
    GIC,
    GOC
} SPMODE;

SPMODE calMode()
{
    if (ALL(runLog[runTime]) && runLog[runTime - 4]&0x18U && ALL(runLog[runTime - 9]))
        return STOP;
    if (MID(runLog[runTime]) && ALL(runLog[runTime - 3]) && MID(runLog[runTime - 8]))
        return CROSS;
    if (NON(runLog[runTime]) && BIC(runLog[runTime - 5]) && LFT(runLog[runTime - 15]))
        return GIC;
    if ((LFT(runLog[runTime])||MID(runLog[runTime])) && ((runLog[runTime - 5] & 0x3FU)==0x3FU) && LFT(runLog[runTime - 10]))
        return GOC;
    return NORMOL;
}

void justrun(void)
{
    int i = 0;
    AGV_GetStates(1, AGV_Data);
    SWITCHPOS;
    runTime++;
    runLog[runTime] = pospos;
	
    getLocation();
    anglePID.actrul = location;
    dirSet = (int32_t)PID_LocCal(&anglePID, PID_P | PID_D);

    for (i = 0; i < 8; i++)
        printf("%d\t", AGV_Data[i]);
    printf("\t");

    /*
	if (spMode == 0 && ALL) //ALL situation is already in special mode. 		Take spMode to 1
	{
		lastspMode=spMode;
		spMode = 1;
		spTime = runTime;
	}
	
	if (spMode == 0 && BIC) //ALL situation is already in special mode. 		Take spMode to 6
	{
		lastspMode=spMode;
		spMode = 6;
		spTime = runTime;
	}

	if (spMode == 1 && MID && TIMERROR >= 5) //ALL---MID  situation is a cross or a cycle.		Take spMode to 2
	{
		spMode = 2;
		spTime = runTime;
	}

	if (spMode == 2 && MID && TIMERROR >= 15) //ALL---MID---MID situation is a cross.				Take spMode to 0
	{
		spMode = 0;
		spTime = runTime;
	}

	if (spMode == 6 && NON && TIMERROR >= 10) //ALL---NON situation is going into the cycle.		Take spMode to 3
	{
		lastspMode=spMode;
		spMode = 3;
		spTime = runTime;
	}

	if ((spMode == 6||spMode == 1) && LFT && TIMERROR >= 6) //ALL---LFT situation is going out of the cycle.	Take spMode to 4
	{
		lastspMode=spMode;
		spMode = 4;
		spTime = runTime;
	}

	if (spMode == 2 && ALL && TIMERROR >= 5) //ALL---MID---ALL situation is location to stop.	Take spMode to 5
	{
		lastspMode=spMode;
		spMode = 5;
		spTime = runTime;
	}
*/

    if(spMode==0)
    {
		spMode = calMode();
		spTime = runTime;
		if(spMode==STOP)
		{
			FinalTime=runTime-stopPassTime;
			stopPassTime=runTime;
		}
    }

    printf("%d\t%d\t%d\t%d\t\r\n", spMode, runTime, TIMERROR, pospos);

    if (spMode == CROSS)
    {
        if (TIMERROR >= 2)
            spMode = 0;
    }

    if (spMode == GIC) //Is time to go into a cycle.
    {
        if (TIMERROR >= 25)
            spMode = 0;


        dirSet = -300;
        location=80;
        Servo_DirSet(900 - dirSet);
//        rMotorControl(targetSpeed - abs((int16_t)(dirSet / 4)));
//        lMotorControl(targetSpeed - abs((int16_t)(dirSet / 4)));
        rMotorControl(0);
        lMotorControl(0);
        return;
    }

    if (spMode == GOC&&runTime>1000)
    {
        if (TIMERROR >= 100)
            spMode = 0;


        dirSet = -300;
        location = 80;
        Servo_DirSet(900 - dirSet);
        rMotorControl(targetSpeed - abs((int16_t)(dirSet / 7)));
        lMotorControl(targetSpeed - abs((int16_t)(dirSet / 7)));
        return;
    }

//    if (spMode == STOP)
//    {
//        if (runTime>1000)
//        {
//            rMotorControl(0);
//            lMotorControl(0);

//            Servo_DirSet(900);
//            TIM_ITConfig(TIM3, TIM_IT_Update, DISABLE);
//            return;
//        }
//    }
	
    if (spMode != 0 && TIMERROR >= 100)
    {
        spMode = 0;
        spTime = runTime;
    }

    Servo_DirSet(900 - dirSet);

    rMotorControl(targetSpeed - abs((int16_t)(dirSet / 7)));
    lMotorControl(targetSpeed - abs((int16_t)(dirSet / 7)));

	
	if(runTime>=5000)
		runInit();
	return;
}

void rMotorControl(int32_t rmOutput)
{
    TIM_SetCompare1(TIM4, 0);        //电机1+
    TIM_SetCompare2(TIM4, rmOutput); //电机1-
}

void lMotorControl(int32_t lmOutput)
{
    TIM_SetCompare3(TIM4, lmOutput); //电机2+
    TIM_SetCompare4(TIM4, 0);        //电机2-
}

void stateDisplay()
{
    int i = 0;
    for (i = 0; i < 8; i++)
        OLED_ShowNum(0, i, AGV_Data[i], 4, 12);
    OLED_ShowNum(50, 0, location, 4, 12);
    OLED_ShowNum(50, 2, dirSet, 4, 12);
    OLED_ShowNum(50, 4, targetSpeed - abs(dirSet / 5), 4, 12);
    OLED_ShowNum(50, 6, spMode, 4, 12);
    OLED_ShowNum(100, 6, FinalTime, 4, 12);
}
